#include "rm_motor.h"
#include "M3508_SMC.h"

void smc_init(smc_controller* smc,float k)
{
    smc->k = k;
}

void smc_data_init(DJI_Motor_ControlTypeDef* motor, uint8_t motor_type, uint8_t control_type, uint8_t MOTOR_GROUP)
{
    if (MOTOR_GROUP == LOW)
	{
		if (control_type == Nom_Ctrl)
		{
			if (motor_type == 8 || motor_type == 9)
			{
				smc_init(motor->motor_s_smc,300);
                smc_init(motor->motor_s_smc+1,300);
                smc_init(motor->motor_s_smc+2,300);
                smc_init(motor->motor_s_smc+3,300);
                smc_init(motor->motor_p_smc,300);
                smc_init(motor->motor_p_smc+1,300);
                smc_init(motor->motor_p_smc+2,300);
                smc_init(motor->motor_p_smc+3,300);
			}
			else if (motor_type == 10)
			{
				smc_init(motor->motor_s_smc,300);
                smc_init(motor->motor_s_smc+1,300);
                smc_init(motor->motor_s_smc+2,300);
                smc_init(motor->motor_s_smc+3,300);
                smc_init(motor->motor_p_smc,300);
                smc_init(motor->motor_p_smc+1,300);
                smc_init(motor->motor_p_smc+2,300);
                smc_init(motor->motor_p_smc+3,300);
			}
		}
		if (control_type == MIT_Ctrl)
		{
			smc_init(motor->motor_s_smc,300);
            smc_init(motor->motor_s_smc+1,300);
            smc_init(motor->motor_s_smc+2,300);
            smc_init(motor->motor_s_smc+3,300);
            smc_init(motor->motor_p_smc,300);
            smc_init(motor->motor_p_smc+1,300);
            smc_init(motor->motor_p_smc+2,300);
            smc_init(motor->motor_p_smc+3,300);
		}
	}
	else if (MOTOR_GROUP == HIGH)
	{
		if (control_type == Nom_Ctrl)
		{
			if (motor_type == 8 || motor_type == 9)
			{
				smc_init(motor->motor_s_smc,300);
                smc_init(motor->motor_s_smc+1,300);
                smc_init(motor->motor_s_smc+2,300);
                smc_init(motor->motor_s_smc+3,300);
                smc_init(motor->motor_p_smc,300);
                smc_init(motor->motor_p_smc+1,300);
                smc_init(motor->motor_p_smc+2,300);
                smc_init(motor->motor_p_smc+3,300);
			}
			else if (motor_type == 10)
			{
				smc_init(motor->motor_s_smc,300);
                smc_init(motor->motor_s_smc+1,300);
                smc_init(motor->motor_s_smc+2,300);
                smc_init(motor->motor_s_smc+3,300);
                smc_init(motor->motor_p_smc,300);
                smc_init(motor->motor_p_smc+1,300);
                smc_init(motor->motor_p_smc+2,300);
                smc_init(motor->motor_p_smc+3,300);
			}
		}
		if (control_type == MIT_Ctrl)
		{
			smc_init(motor->motor_s_smc,300);
                smc_init(motor->motor_s_smc+1,300);
                smc_init(motor->motor_s_smc+2,300);
                smc_init(motor->motor_s_smc+3,300);
                smc_init(motor->motor_p_smc,300);
                smc_init(motor->motor_p_smc+1,300);
                smc_init(motor->motor_p_smc+2,300);
                smc_init(motor->motor_p_smc+3,300);
		}
	}
}

void rm_motor_smc_init(DJI_Motor_ControlTypeDef* motor, uint8_t motor_type, CAN_HandleTypeDef* hcan, uint8_t MOTOR_GROUP)
{
	HAL_Delay(1000);
	if (MOTOR_GROUP == LOW)
	{
		if (motor_type == 8 || motor_type == 9)
		{
			rm_motor_init(&(motor->RM_CAN), 0x200, hcan);
		}
		else if (motor_type == 10)
		{
			rm_motor_init(&(motor->RM_CAN), 0x1ff, hcan);
		}
	}
	else if (MOTOR_GROUP == HIGH)
	{
		if (motor_type == 8 || motor_type == 9)
		{
			rm_motor_init(&(motor->RM_CAN), 0x1ff, hcan);
		}
		else if (motor_type == 10)
		{
			rm_motor_init(&(motor->RM_CAN), 0x2ff, hcan);
		}
	}

	rm_motor_cmd_max_config(&(motor->RM_CAN), PID_MAX, PID_MAX, PID_MAX, PID_MAX);
    for (int i = 0; i < 4; i++)
	{
		smc_data_init(motor->motor_s_smc + i, 350,500);
		smc_data_init(motor->motor_p_smc + i, 350,500);
	}
	motor_smc_data_init(motor, motor_type, Nom_Ctrl, MOTOR_GROUP);
	rm_motor_cmd_refresh(&(motor->RM_CAN), 0, 0, 0, 0);
	rm_motor_send_msg(&(motor->RM_CAN));
}

void rm_motor_speed_smc(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t target_speed)
{
    float error = target_speed - motor->RM_CAN.motor_data[id].speed;
	smc_error_input(motor->motor_s_pid + id, error);
	motor->cmd_buffer[id] = smc_output(motor->motor_s_pid + id);

	//M2006无温度回传 3508 6020 可用
	if (motor->RM_CAN.motor_data[id].temperature >= 50)
	{
		motor->cmd_buffer[id] = 0;
	}
}

void rm_motor_speed_smc_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed)
{
	motor_position_cal(motor,id);
	motor_3508_speed_smc(motor, id, speed);
}

void speed_smc_ctrl(DJI_Motor_ControlTypeDef* motor, int16_t speed_1, int16_t speed_2, int16_t speed_3, int16_t speed_4, uint8_t motor_type)
{
	float speed[4] = { speed_1,speed_2,speed_3,speed_4 };
	for (int i = 0; i < 4; i++)
	{
		motor->M3508[i].exp_speed = speed[i];
	}
	for (int i = 0; i < 4; i++)
	{
		rm_motor_speed_smc_ctrl(motor, i, motor->M3508[i].exp_speed);
	}
	rm_motor_cmd_refresh(&(motor->RM_CAN), motor->cmd_buffer[0], motor->cmd_buffer[1], motor->cmd_buffer[2], motor->cmd_buffer[3]);
	rm_motor_send_msg(&(motor->RM_CAN));
}